/**
  ******************************************************************************
  * @file     : Can_Motor_user.cpp
  * @brief    : can电机的用户层代码源文件
  ******************************************************************************
  * @attention
  * 请在main.c初始化代码段调用can_motor_init即可完成所有关于can电机的初始化(请在rtos内核初始化之后调用目前放在freertos.c的MX_FREERTOS_Init里面调用)
  * 在can_motor_pid_init函数中修改各个电机的pid参数
  * 在can_motor_measure_init里面配置各个电机的初始模式
  */
#include "can_motor_user.h"
#include "bsp_can.h"
#include "pid.h"
#include "can_motor_task.h"

/*	外部变量&头文件链接区 BEGIN	*/

/*	外部变量&头文件链接区 END	*/

//定义电机类
Can_motor   m_0x201_can1;       //底盘3508
Can_motor   m_0x202_can1;       //底盘3508
Can_motor   m_0x203_can1;       //底盘3508
Can_motor   m_0x204_can1;       //底盘3508
Can_motor   m_0x205_can1;       //防守3508

Can_motor   m_0x201_can2;       //运球3508
Can_motor   m_0x202_can2;       //运球3508
Can_motor   m_0x203_can2;       //投篮3508
Can_motor   m_0x204_can2;       //投篮3508
Can_motor   m_0x205_can2;       //投篮2006

static void can_motor_measure_init();
static void can_motor_pid_init();

/*		       文件内调用函数(不做对外暴露的接口) START	      	*/
/**
 * @brief 配置好所有电机的参数,默认放在can_motor_init中调用
 */
static void can_motor_measure_init()
{
	//底盘
    m_0x201_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x201);
    m_0x202_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x202);
    m_0x203_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x203);
    m_0x204_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x204);
    //防守
    m_0x205_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x205);
	//运球
    m_0x201_can2.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x201);
    m_0x202_can2.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x202);
    //发射
    m_0x203_can2.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x203);
    m_0x204_can2.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x204);
    m_0x205_can2.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x205);
}

/**
 * @brief 配置好所有电机的pid参数,默认放在can_motor_init中调用
 */
static void can_motor_pid_init()
{
	/*	底盘电机pid start */
	{
		/*	底盘电机速度环pid	*/
		{
            const uint32_t MAX_CURENT = 12000;		//速度环最大电流输出
            float p = 5.0f, i = 0.05f, d = 0.2f, f = 0.0f;
            float intergral_lim = 5000, deadzone = 3;
			
			m_0x201_can1.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                            pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                           MAX_CURENT,	intergral_lim,
                                           deadzone,
                                           p ,i, d, f);

			m_0x202_can1.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                            pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                        MAX_CURENT + 3000,	intergral_lim + 6000,
                                        deadzone,
                                        p*1.5f, i, d, f + 0.2f);

			m_0x203_can1.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                            pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
				                        MAX_CURENT,	intergral_lim,
                                        deadzone,
                                        p, i, d, f);

			m_0x204_can1.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                        MAX_CURENT,	intergral_lim,
                                        deadzone,
                                        p *0.8f, i, d, f);
		}
	}
	/*	底盘电机pid end	*/
	
	/*	运球电机pid start */
	{
		/*	运球电机速度环pid	*/
		{
			const float MAX_CURENT = 10000;		//速度环最大电流输出
			float p = 2, i = 0.1, d = 0.5, f = 0;
			float intergral_lim = 5000, deadzone = 5;
			
			m_0x201_can2.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                             MAX_CURENT,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
            m_0x202_can2.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                             MAX_CURENT,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
		}
	}
	/*	运球电机pid end	*/
	
	/*	发射电机pid start */
	{
		/*	3508速度环pid	*/
		{
            const float MAX_CURENT = 10000;		//速度环最大电流输出
            float p = 2, i = 0.0, d = 0.0, f = 0;
            float intergral_lim = 5000, deadzone = 2;

            m_0x203_can2.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_EN,
                                             pid_t::PID_ENUM::DEADBAND_NOT_CLEAR_I,
                                             MAX_CURENT,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
            m_0x204_can2.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_EN,
											 pid_t::PID_ENUM::DEADBAND_NOT_CLEAR_I,
                                             MAX_CURENT,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
		}
		/*	3508角度环pid	*/
		{
			const float MAX_SPEED = 3000;		//角度环最大速度输出
            float p = 3.5, i = 0.0, d = 0.01, f = 0;
			float intergral_lim = 5000, deadzone = 0.01;

            m_0x203_can2.apid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_NOT_CLEAR_I,
                                             MAX_SPEED,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
            m_0x204_can2.apid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_NOT_CLEAR_I,
                                             MAX_SPEED,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
		}
		/*	2006速度环pid	*/
		{
			const float MAX_CURENT = 10000;		//速度环最大电流输出
			float p = 2, i = 0.1, d = 0.5, f = 0;
			float intergral_lim = 5000, deadzone = 5;

            m_0x205_can2.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_EN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                             MAX_CURENT,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);

		}
		/*	2006角度环pid	*/
		{
			const float MAX_SPEED = 2000;		//角度环最大速度输出
			float p = 10, i = 0, d = 20, f = 0;
			float intergral_lim = 2000, deadzone = 0.1;
            m_0x205_can2.apid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                             MAX_SPEED,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);

		}
	}
	/*	发射电机pid end	*/
	
	/*	防守电机pid start */
	{
		/*	防守电机速度环pid	*/
		{
            const float MAX_CURENT = 10000;		//速度环最大电流输出
            float p = 2, i = 0.1, d = 0.5, f = 0;
            float intergral_lim = 5000, deadzone = 5;

            m_0x205_can1.spid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_EN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                             MAX_CURENT,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
		}
		/*	防守电机角度环pid	*/
		{
            const float MAX_SPEED = 4000;		//角度环最大速度输出
            float p = 10, i = 0, d = 20, f = 0;
            float intergral_lim = 2000, deadzone = 0.1;
            m_0x205_can1.apid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                             pid_t::PID_ENUM::D_FIRST_DISEN,
                                             pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                             MAX_SPEED,	intergral_lim,
                                             deadzone,
                                             p ,i, d, f);
		}
	}
	/*	防守电机pid end	*/
}

/*		    文件内调用函数(不做对外暴露的接口) END	    	*/


/************************************************************/
/*	                      对外暴露接口                      	*/
/************************************************************/
/**
 * @brief 做好一切与can和电机有关的初始化操作
 */
extern "C" void can_motor_init()
{
    //初始化电机参数
    can_motor_measure_init();
    //初始化电机pid参数
    can_motor_pid_init();
	//开电机任务
	can_motor_task_init();
}





